#include <WiFi.h>
#include <WebServer.h>
#include <WebSocketsServer.h>  //https://github.com/Links2004/arduinoWebSockets
#include "DRV8833Controller.h"

// WiFi 配置
#define WS_PORT 81
const char *ssid = "toycar";
const char *password = "12345678";

// 电机控制器初始化（根据实际引脚连接修改）
const int forward_pin = 0;     // AIN1 黑色电机线
const int backward_pin = 1;    // AIN2 绿色电机线
const int turn_left_pin = 7;   // BIN2 绿色电机线
const int turn_right_pin = 6;  //  BIN1黑色电机线

DRV8833Controller robot(forward_pin, backward_pin, turn_left_pin, turn_right_pin);

WebServer server(80);
WebSocketsServer webSocket = WebSocketsServer(WS_PORT);

// 嵌入式HTML页面（含速度显示UI）
const char *htmlPage = R"rawliteral(
<!DOCTYPE html>
<html>
<head>
<meta name="viewport" content="width=device-width, initial-scale=1">
  <title>ToyCar</title>
  <style>
   html { font-family: Helvetica; display: inline-block; margin: 0px auto; text-align: center;}
   .button { background-color: #195B6A; border: none; color: white; padding: 3px 10px;
            width:150px; height: 30px;text-decoration: none; font-size: 18px; margin: 2px; cursor: pointer;}
  </style>
</head>
<body>
  <h1 align=center>ToyCar</h1>
  <div>
    <p><button class="button" onclick="sendCmd('F')">GO</button></p>
    <p><button class="button" onclick="sendCmd('S')">STOP</button></p>
    <p><button class="button" onclick="sendCmd('B')">Back</button></p>
    <p><button class="button" onclick="sendCmd('L')">Left</button></p>
    <p><button class="button" onclick="sendCmd('C')">Center</button></p>
    <p><button class="button" onclick="sendCmd('R')">Right</button></p>
    <p><button class="button" onclick="sendCmd('+')">Speed Up</button>&ensp;&ensp;
      <button class="button" onclick="sendCmd('-')">Speed Down</button></p>
  <div> 
   </div>
  <h2>Robot Speed: <span id="speed">0</span></h2>
 </div>
  <script>
    const ws = new WebSocket('ws://' + window.location.hostname + ':81/');
    
    function sendCmd(cmd) {
      if (ws.readyState === WebSocket.OPEN) {
        ws.send(cmd);
      }
    }

    ws.onmessage = (event) => {
      const data = JSON.parse(event.data);
      if (data.type === "status") {
        document.getElementById("speed").textContent = data.speed;
      }
    };

    ws.onopen = () => console.log("WebSocket 已连接");
    ws.onerror = (err) => console.log("WebSocket 错误:", err);
  </script>
</body>
</html>)rawliteral";

void handleRoot() {
  server.send(200, "text/html", htmlPage);
}

// WebSocket 事件处理
void webSocketEvent(uint8_t num, WStype_t type, uint8_t *payload, size_t length) {
  switch (type) {
    case WStype_TEXT:
      {
        String cmd = (char *)payload;
        ROBOT_ACTION_CODE cur_robot_cmd = robot.state;
        if (cmd == "F") {
          cur_robot_cmd = ROBOT_FORWARD;
        } else if (cmd == "S") {
          cur_robot_cmd = ROBOT_STOP;
        } else if (cmd == "B") {
          cur_robot_cmd = ROBOT_BACKWARD;
        } else if (cmd == "L") {
          robot.action(ROBOT_TURN_LEFT);
        } else if (cmd == "R") {
          robot.action(ROBOT_TURN_RIGHT);
        } else if (cmd == "C") {
          robot.action(ROBOT_CENTER);
        } else if (cmd == "+") {
          cur_robot_cmd = ROBOT_SPEED_UP;
        } else if (cmd == "-") {
          cur_robot_cmd = ROBOT_SPEED_DOWN;
        }
        Serial.print("robot cmd: ");
        Serial.println(cur_robot_cmd);
        if (cur_robot_cmd != robot.state) {
          robot.action(cur_robot_cmd);
        };
        break;
      }
    case WStype_DISCONNECTED:
      robot.action(ROBOT_STOP);
      break;
  }
}

// 获取Robot状态（JSON格式）
void sendRobotStatus() {
  String json = String() + "{\"type\":\"status\"," + "\"speed\":" + robot.speed + "}";
  webSocket.broadcastTXT(json);
}

void setup() {
  Serial.begin(115200);

  // You can remove the password parameter if you want the AP to be open.
  // a valid password must have more than 7 characters
  if (!WiFi.softAP(ssid, password)) {
    log_e("Soft AP creation failed.");
    while (1)
      ;
  }

  IPAddress myIP = WiFi.softAPIP();
  Serial.print("AP IP address: ");
  Serial.println(myIP);

  server.on("/", handleRoot);
  server.begin();

  webSocket.begin();
  webSocket.onEvent(webSocketEvent);
}

void loop() {
  server.handleClient();
  webSocket.loop();
  // 每200ms发送一次状态
  static unsigned long lastUpdate = 0;
  if (millis() - lastUpdate > 200) {  // 每200ms发送一次状态
    sendRobotStatus();
    lastUpdate = millis();
  }
}